/**
 * TODO
 * 还没有添加引力场，
 * 引力场是为了无人机在飞行过程中能够
 * 朝向目标位置输出正常的动力，目前先
 * 调能在静态悬停下进行躲避的工作
 */

#ifndef __ITEM__
#define __ITEM__

#include "std_msgs/Float64MultiArray.h"
#include <eigen3/Eigen/Eigen>
#include <iostream>
#include <map>
#include <ros/ros.h>
#include <vector>

#define DEBUG 1
#define INF   4294967295

using namespace Eigen;
using namespace std;

/**
 * 存储每个障碍物信息
 */
class Item
{
public:
    // 以相机坐标系FLU为标准
    // 目标物体的速度和位置
    Vector3d v;
    Vector3d position;

private:
    // 初始参数
    double K, gamma, n0, lambda, k_r_t;

    // 上次检测到的时间
    ros::Time t_detection;

    // 无人机航向角，目前默认为0
    Vector3d theta_quadrotor;

public:
    Item() {}

    Item(double _K, double _gamma, double _n0, double _lambda, double _k_r_t);

    Vector3d calculateForce();


private:
    double get_K();

    // TODO
    // 设置无人机航向角，目前认为是悬停状态，不需要设置。
    Vector3d get_theta();

    double get_dis_coefficient();

    double get_speed() const { return v.norm(); }
};


/**
 * 接收障碍物话题
 */
class Items
{
private:
    // 初始参数
    double K, gamma, n0, lambda, k_r_t;
    double F;

    map<int, Item> data;

    ros::Publisher force_pub;

public:
    Vector3d force;


public:
    Items() {}

    Items(ros::NodeHandle& nh, double _K, double _gamma, double _n0, double _lambda, double _k_r_t, double _F);

    void feed(const std_msgs::Float64MultiArrayConstPtr& msg);

private:
    void calculate();

};

#endif
